/*
 * Copyright (c) 2009, Katholieke Universiteit Leuven
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 
 *     * Redistributions of source code must retain the above copyright notice,
 *       this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice,
 *       this list of conditions and the following disclaimer in the documentation
 *       and/or other materials provided with the distribution.
 *     * Neither the name of the Katholieke Universiteit Leuven nor the names of
 *       its contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @addtogroup vector
 * @{
 */
/**
 * @file
 * Implementation for the vector library.
 * @author
 * Wouter Horré <wouter.horre@cs.kuleuven.be>
 */

#include "lib/vector.h"

#include <string.h>

#ifdef LOOCI_DEBUG
#define DEBUG printf
#include <stdio.h>
#else
#define DEBUG(...)
#endif

static void vector_shrink(struct vector * vector);
static void vector_blockinit(struct vector * vector, struct vector_block * block);
static u8_t vector_grow(struct vector * vector, struct vector_block * last);

static u8_t initialized = 0;

static void vector_shrink(struct vector * vector)
{
  DEBUG("[Vector] Shrinking vector of size %u\r\n", vector->len);
  u8_t nb_blocks = ((vector->len-1) / VECTOR_BLOCKSIZE) + 1;
  DEBUG("[Vector] Number of blocks is %u\r\n", nb_blocks);
  if(vector->len == 0 || nb_blocks == 1) { // nb_blocks probably isn't correct if len == 0
    // we can't remove our first block
    return;
  }
  // skip to last used block
  struct vector_block * current = vector->first;
  while(nb_blocks > 1) {
    current = (struct vector_block *) MMEM_PTR(current->next);
    nb_blocks--;
  }
  // now check whether there is an unused block after that one
  if(current->next != NULL) {
    DEBUG("[Vector] Freeing a block\r\n");
    mmem_free(current->next);
    current->next = NULL;
  }
}

static void vector_blockinit(struct vector * vector, struct vector_block * block)
{
  int i;
  for(i=0; i < VECTOR_BLOCKSIZE * vector->elementsize; ++i) {
    block->elements[i] = 0;
  }
  block->next = NULL;
}

static u8_t vector_grow(struct vector * vector, struct vector_block * last)
{
  DEBUG("[Vector] Growing vector of size %u\r\n", vector->len);
  if(last == NULL) {
    // skip to last
    last = vector->first;
    while(last->next != NULL) {
      last = (struct vector_block *) MMEM_PTR(last->next);
    }
  }
  // try to allocate a successor
  if(!mmem_alloc(&(last->mmem_next), sizeof(struct vector_block) + vector->elementsize * VECTOR_BLOCKSIZE)) {
    // failure -> return FAIL
    DEBUG("[Vector] Could not grow!\r\n");
    return VECTOR_FAIL;
  }
  // succesfully allocated successor
  // update last's next pointer
  last->next = &(last->mmem_next);
  // initialize new block
  vector_blockinit(vector, (struct vector_block *)MMEM_PTR(last->next));
  return VECTOR_OK;
}

void vector_libinit()
{
  if(initialized == 0) {
    mmem_init();
  }
  initialized = 1;
}

void vector_init(struct vector * vector)
{
  vector->len = 0;
  vector_blockinit(vector, vector->first);
}

u8_t vector_len(struct vector * vector)
{
  return vector->len;
}

u8_t vector_add(struct vector * vector, void * element)
{
  struct vector_block * block = vector->first;
  u8_t len = vector->len;
  //skip to last block
  while(block->next != NULL) {
    block = (struct vector_block *)MMEM_PTR(block->next);
    len -= VECTOR_BLOCKSIZE;
  }
  if(len >= VECTOR_BLOCKSIZE) {
    DEBUG("[Vector] We will have to grow because len is still %u\r\n", len);
    // we will have to create a successor and skip to it
    if(vector_grow(vector, block)==VECTOR_FAIL) {
      // could not grow
      return VECTOR_FAIL;
    }
    // skip to new successor
    block = (struct vector_block *)MMEM_PTR(block->next);
    len -= VECTOR_BLOCKSIZE;
  }
  // add element
  memcpy(block->elements + vector->elementsize * len, element, vector->elementsize);
  vector->len++;
  return VECTOR_OK;
}

void * vector_get(struct vector * vector, u8_t index)
{
  if(index >= vector->len) {
    // index too large
    return NULL;
  }
  struct vector_block * block = vector->first;
  while(index >= VECTOR_BLOCKSIZE) {
    index -= VECTOR_BLOCKSIZE;
    if(block->next == NULL) {
      // index too large! return NULL -> we shouldn't get here, but you never know
      return NULL;
    }
    block = (struct vector_block *)MMEM_PTR(block->next);
  }
  return block->elements + vector->elementsize * index;
}

void vector_remove(struct vector * vector, u8_t index) {
  if(index >= vector->len) { // sanity check
    DEBUG("[Vector] Trying to remove an element which is not here\r\n");
    return;
  }
  // find correct block
  struct vector_block * block = vector->first;
  while(index >= VECTOR_BLOCKSIZE) {
    if(block->next == NULL) {
      DEBUG("[Vector] Index too large, but we shouldn't get here\r\n");
      // index too large! return -> we shouldn't get hre, but you never know
      return;
    }
    block = (struct vector_block *) MMEM_PTR(block->next);
    index -= VECTOR_BLOCKSIZE;
  }
  // remove element
  vector->len--;
  DEBUG("[Vector] Reduced length to %u\r\n", vector->len);
  // shift all further elements forward
  do {
    // copy within the block
    while(index < VECTOR_BLOCKSIZE-1) {
      DEBUG("[Vector] Shifting element with index %u\r\n", index);
      memcpy(block->elements + (index * vector->elementsize), block->elements + ((index+1) * vector->elementsize), vector->elementsize);
      ++index;
    }
    if(block->next!=NULL) { // if there is a next block
      // copy first element of next block into last on of this block
      struct vector_block * nextblock = (struct vector_block *) MMEM_PTR(block->next);
      DEBUG("[Vector] Shifting first element of next block\r\n");
      memcpy(block->elements + (index * vector->elementsize), nextblock->elements, vector->elementsize);
      // reset index and make nextblock current block
      index = 0;
      block = nextblock;
    } else { // if there is no next block
      // set last element to zero
      DEBUG("[Vector] Zeroing last element of the block\r\n");
      memset(block->elements + (index * vector->elementsize), 0, vector->elementsize);
      break;
    }
  } while (block != NULL);
  // check whether we can free a block
  if((vector->len % VECTOR_BLOCKSIZE) == 0) {
    DEBUG("[Vector] Calling shrink\r\n");
    vector_shrink(vector);
  }
}

/** @} */
